#include "plan_and_control/trajectory_replan_node.h"

TrajectoryReplanNode::TrajectoryReplanNode(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) 
:nh_(nh), nh_private_(nh_private), got_circle_flag_(false) {
    
    fsm_state = TAKEOFF; // 状态机初始化 INIT
    last_fsm_state = INIT;
    takeoff_client_ = nh_.serviceClient<airsim_ros::Takeoff>("/airsim_node/drone_1/takeoff");//get params from sever

    it_ = std::make_unique<image_transport::ImageTransport>(nh_);
    // circle_info=nh_.subscribe<darknet_ros_msgs::BoundingBoxes>("/darknet_ros/bounding_boxes",1,std::bind(&TrajectoryReplanNode::findcircle, this, std::placeholders::_1));   
    // depth_image_sub_ = it_->subscribe("airsim_node/drone_1/front_center/DepthPlanar", 1, 
    //                     std::bind(&TrajectoryReplanNode::depthImageCallback, this,  std::placeholders::_1));
    // odom_sub_ =
    //     nh_.subscribe<nav_msgs::Odometry>("/airsim_node/drone_1/odom_local_ned",1, &TrajectoryReplanNode::odomCallback,this);
    
    pos_sub_ =
        nh_.subscribe<geometry_msgs::PoseStamped>("/airsim_node/drone_1/debug/pose_gt",1, &TrajectoryReplanNode::posCallback,this);
    vel_sub_ = 
        nh_.subscribe<airsim_ros::VelData>("/drone_rl/vel_est",1, &TrajectoryReplanNode::velCallback,this);
    circlePos_sub_ = 
        nh_.subscribe<airsim_ros::CirclePoses>("/airsim_node/drone_1/debug/circle_poses_gt",1, &TrajectoryReplanNode::circlePosCallback,this);
    desiredStates_pub_ = 
        nh_.advertise<airsim_ros::DesiredStates>("/reference/desiredStates", 1);
    circle_detect_sub_ = 
        nh_.subscribe<custom_msgs::CircleRecog>("/Circle/circle_detection", 1, &TrajectoryReplanNode::circleDetectCallback,this);
    // minijerk_tricker_sub_ =
    //     nh_.subscribe<airsim_ros::MinijerkTricker>("/drone_rl/minijerk_tricker",1, &TrajectoryReplanNode::minijerkTrickerCallback,this);
    
    //  可视化
    traj_vis_pub_ = nh_.advertise<visualization_msgs::Marker>("/trajectory_vis", 10);
    cmd_vis_pub_ = nh_.advertise<visualization_msgs::Marker>("/cmd_vis",10);
    desiredPose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/desiredPose", 10);
    currentPose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/currentPose", 10);
    
    // 15hz
    traj_proc_timer_ = nh_.createTimer(ros::Duration(0.05), &TrajectoryReplanNode::FSMProcess, this);
    circle_check_timer_ = nh_.createTimer(ros::Duration(0.1), &TrajectoryReplanNode::circleCheckProcess, this);
    //cout << "waypoint init failed!" << endl;
    
    // takeoff_client_ = nh_.serviceClien=t<airsim_ros::Takeoff>("/airsim_node/drone_1/takeoff");//get params from sever
    nh_private_.getParam("max_vel", max_vel_);
    nh_private_.getParam("max_acc", max_acc_);
    nh_private_.getParam("scalar_1", scalar_1);
    nh_private_.getParam("scalar_2", scalar_2);
    nh_private_.getParam("end_vel_x", end_vel_x);
    nh_private_.getParam("end_vel_y", end_vel_y);
    nh_private_.getParam("end_vel_z", end_vel_z);

    // nh_private_.getParam("waypoint_num", waypoint_num_);
    // waypoints_.resize(waypoint_num_, 3);
    // cout << "waypoint number:" << waypoint_num_ << endl;

    // for (int i = 0; i < waypoint_num_; ++i) {
    //     nh_private_.getParam("waypoint" + to_string(i) + "/x", waypoints_(i,0));
    //     nh_private_.getParam("waypoint" + to_string(i) + "/y", waypoints_(i,1));
    //     nh_private_.getParam("waypoint" + to_string(i) + "/z", waypoints_(i,2));
    // }


    // cout << "waypoints:" << waypoints_ << endl;
    // airsim_ros::Takeoff takeoff;
    // takeoff.request.waitOnLastTask = 1;
    // takeoff_client_.call(takeoff);

    // trajectoryOK_ = trajectoryGenerate(waypoints_);
}

TrajectoryReplanNode::~TrajectoryReplanNode() {}

void TrajectoryReplanNode::FSMProcess(const ros::TimerEvent &e){
    
    switch (fsm_state) {
    case INIT: {

        break;
    }
    case TAKEOFF: {
        airsim_ros::Takeoff takeoff;
        takeoff.request.waitOnLastTask = 1;
        if (takeoff_client_.call(takeoff)){
            ROS_INFO("TAKEN OFF!!");
            changeFSMstate(FLY_TO_CIRCLE_PLAN);
        }
        break;
    }
    case FLY_TO_CIRCLE_PLAN: { //飞到环前
        setWaypointsForMinijerk(odom_pos_, circle_target_pos_);
        bool traj_ok = trajectoryGenerate(waypoints_);
        if (traj_ok){    //刷新标志位
            trajectory_count_ = 0;
            time_sum_ = 0.0;
            changeFSMstate(PUB_TRAJ);
        }
        break;
    }
    case PASS_CIRCLE_REPLAN: { //穿环重规划
        setWaypointsForMinijerk(odom_pos_, circle_target_pos_);
        if (trajectoryGenerate(waypoints_)){ //刷新标志位
            trajectory_count_ = 0;
            time_sum_ = 0.0;
            changeFSMstate(PUB_TRAJ);
        } 
        break;
    }
    case PUB_TRAJ: {
        desiredStatesPub(); // 将minimum jerk的规划结果发布给几何控制器控制
        break;
    }

    default:
        break;
    }
    // if (minijerk_tricker_ && first_minijerk_){
    //     trajectoryOK_ = trajectoryGenerate(waypoints_);
    //     if (trajectoryOK_){
    //         first_minijerk_ = false;
    //         // desiredStatesPub();
    //     }
    // }
    // if (!minijerk_tricker_){
    //     first_minijerk_ = true;
    //     waypoints_.setZero();
    //     odom_vel_.setZero();

    //     trajectory_count_ = 0;
    //     time_sum_ = 0.0;
    //     start_time_ = ros::Time::now();
    //     final_time_ = ros::Time::now();
    //     trajectoryOK_ = false;
    // }
    // else if (trajectoryOK_){
    //     desiredStatesPub();
    // }
}


void TrajectoryReplanNode::changeFSMstate(STATE state){
    last_fsm_state = fsm_state;
    fsm_state = state;
    static string state_str[8] = {"INIT", "TAKEOFF", "PASS_CIRCLE_REPLAN", "FLY_TO_CIRCLE_PLAN", "PUB_TRAJ"};
    cout << "[StateChange]" << " from " + state_str[int(last_fsm_state)] + " to " + state_str[int(fsm_state)] << endl;
}

void TrajectoryReplanNode::circleCheckProcess(const ros::TimerEvent &e){
    
    // 判断检测到圆环， 并更新 circle_target_pos_ 进行重规划（还没写）判断圆环检测的信息是最新的
    target_v_ = circle_target_pos_ - odom_pos_;
    if (target_v_.norm() < 5 && last_fsm_state == FLY_TO_CIRCLE_PLAN){ // 阈值待定
        circle_target_pos_ = circle_detect_pos_; // 更新圆心
        changeFSMstate(PASS_CIRCLE_REPLAN);
        target_v_pre_ = target_v_; // 进环前的方向向量
    }

    if (target_v_.dot(target_v_pre_) < 0 && target_v_.norm() < 1) // 穿环完成
    {
        circle_count_++;
        circle_target_pos_ = waypoints_.row(circle_count_); // 更新圆心 
        changeFSMstate(FLY_TO_CIRCLE_PLAN);
    }


}


bool TrajectoryReplanNode::trajectoryGenerate(const Eigen::MatrixXd &waypoints) {
    ros::Time t_start = ros::Time::now();
    cout << "waypoints_:" << waypoints_ << endl; 
    //start final vel and acc 
    Eigen::MatrixXd vel = Eigen::MatrixXd::Zero(2, 3);
    vel(0,0) = odom_vel_(0);
    vel(0,1) = odom_vel_(1);
    vel(0,2) = odom_vel_(2);
    // vel(1,0) = odom_vel_(0);
    // vel(1,1) = odom_vel_(1);
    // vel(1,2) = odom_vel_(2);
    // vel(1,0) = 0.8;
    // vel(1,1) = -4.67;
    // vel(1,2) = 0;
    
    vel(1,0) = end_vel_x;
    vel(1,1) = end_vel_y;
    vel(1,2) = end_vel_z;
    // vel(1,0) = 0.8;
    // vel(1,1) = 1.0;
    // vel(1,2) = -1.0;
    Eigen::MatrixXd acc = Eigen::MatrixXd::Zero(2, 3);
    acc(0,0) = odom_acc_(0);
    acc(0,1) = odom_acc_(1);
    acc(0,2) = odom_acc_(2);
    acc(1,0) = 0;
    acc(1,1) = 0;
    acc(1,2) = 0;
    
    cout << "compute time !" << endl;
    times_ = timeAllocation(waypoints);
    cout << "time vector:" << times_.transpose() << endl;
    coeff_matrix_ = trajPlanWaypoints_->miniJerkTrajGenerate(waypoints, vel, acc, times_);
    cout << "trajectory generate finish!" << endl;
    // cout << "coeff_matrix_:" << coeff_matrix_ << endl;
    final_time_ = start_time_ = ros::Time::now();
    segment_num_ = times_.size();
    for (int i = 0; i < segment_num_; ++i) {
        final_time_ += ros::Duration(times_(i));
    }

    ros::Time t_end = ros::Time::now();
    cout << "traj clac time: " << (t_end -t_start).toSec() << endl;

    return true;
}

/**
 * compute current pos,vel,acc,yaw to controller
 */
void TrajectoryReplanNode::desiredStatesPub() {
    cmd_.header.stamp = ros::Time::now();
    traj_duration_ = (final_time_ - start_time_).toSec();
    //ROS_WARN("timesum:%f:",trajDuration_);
    //cout << "time sum:" << trajDuration_ << endl;
    // double t = max(0.0, (minijerk_tricker_info_->header.stamp - start_time_).toSec());
    // double t = max(0.0, (ros::Time::now() - start_time_).toSec());

    // int i = 0;
    // double time_sum = 0.0;
    // while (t < traj_duration_){

        double t = max(0.0, (ros::Time::now() - start_time_).toSec() - time_sum_);
        if ((ros::Time::now() - start_time_).toSec() >= traj_duration_){
            ROS_INFO("Time over!!!");
            return;
        }
        if ( t >= times_(trajectory_count_) ) {
                time_sum_ += times_(trajectory_count_);
                t = max(0.0, (ros::Time::now() - start_time_).toSec() - time_sum_);
                trajectory_count_++;
            }
        int i = trajectory_count_;
        Eigen::Vector3d desiredPos = trajPlanWaypoints_->getPosition(coeff_matrix_, i, t);
        //cout << "desiredPos:" << desiredPos.transpose() << endl;
        //cout << "odom_Pos:" << odom_pos_.transpose() << endl;

        Eigen::Vector3d desiredVel = trajPlanWaypoints_->getVelocity(coeff_matrix_, i, t);
        //cout << "desiredVel:" << desiredVel.transpose() << endl;
        //cout << "odom_vel:" << odom_vel_.transpose() << endl;
        Eigen::Vector3d desiredAcc = trajPlanWaypoints_->getAcceleration(coeff_matrix_, i, t);
        desired_pos_ = desiredPos;

        cmd_.position.x = desiredPos.x();
        cmd_.position.y = desiredPos.y();
        cmd_.position.z = desiredPos.z();
        cmd_.velocity.x = desiredVel.x();
        cmd_.velocity.y = desiredVel.y();
        cmd_.velocity.z = desiredVel.z();
        cmd_.acceleration.x = desiredAcc.x();
        cmd_.acceleration.y = desiredAcc.y();
        cmd_.acceleration.z = desiredAcc.z();
        cmd_.yaw = atan2(cmd_.velocity.y, cmd_.velocity.x);

        // cmd_.yaw = 0.0;
        // cout << "desiredyaw:" << cmd_.yaw << endl;
        Eigen::Matrix3d currentAttitude = odom_orient_.toRotationMatrix();
        Eigen::Vector3d RPY = currentAttitude.eulerAngles(0,1,2);
        //cout << "currentyaw:" << RPY(0) << endl;
        desiredPose_.header.frame_id = "map";
        desiredPose_.header.stamp = pos_->header.stamp;
        desiredPose_.pose.position.x = desiredPos.x();
        desiredPose_.pose.position.y = desiredPos.y();
        desiredPose_.pose.position.z = desiredPos.z();
        desiredStates_pub_.publish(cmd_);
    // }


    //cout <<odom_->header.stamp << startTime_ << endl;
    //cout << "current:" << t << endl;
    
    // if (t > (traj_duration_)) {
    //     cmd_.position.x = odom_->pose.pose.position.x;
    //     cmd_.position.y = odom_->pose.pose.position.y;
    //     cmd_.position.z = odom_->pose.pose.position.z;

    //     cmd_.velocity.x = 0;
    //     cmd_.velocity.y = 0;
    //     cmd_.velocity.z = 0;

    //     cmd_.acceleration.x = 0;
    //     cmd_.acceleration.y = 0;
    //     cmd_.acceleration.z = 0;

    // } else {
    //     for (int i = 0; i < times_.size(); ++i) {
    //         if (t > times_(i) ) {
    //             t -= times_(i);
    //             ROS_INFO("Time over!");
    //         } else{
    //             Eigen::Vector3d desiredPos = trajPlanWaypoints_->getPosition(coeff_matrix_, i, t);
    //             //cout << "desiredPos:" << desiredPos.transpose() << endl;
    //             //cout << "odom_Pos:" << odom_pos_.transpose() << endl;

    //             Eigen::Vector3d desiredVel = trajPlanWaypoints_->getVelocity(coeff_matrix_, i, t);
    //             //cout << "desiredVel:" << desiredVel.transpose() << endl;
    //             //cout << "odom_vel:" << odom_vel_.transpose() << endl;
    //             Eigen::Vector3d desiredAcc = trajPlanWaypoints_->getAcceleration(coeff_matrix_, i, t);
    //             desired_pos_ = desiredPos;

    //             cmd_.position.x = desiredPos.x();
    //             cmd_.position.y = desiredPos.y();
    //             cmd_.position.z = desiredPos.z();
    //             cmd_.velocity.x = desiredVel.x();
    //             cmd_.velocity.y = desiredVel.y();
    //             cmd_.velocity.z = desiredVel.z();
    //             cmd_.acceleration.x = desiredAcc.x();
    //             cmd_.acceleration.y = desiredAcc.y();
    //             cmd_.acceleration.z = desiredAcc.z();
    //             cmd_.yaw = atan2(cmd_.velocity.y, cmd_.velocity.x);

    //             // cmd_.yaw = 0.0;
    //             // cout << "desiredyaw:" << cmd_.yaw << endl;
    //             Eigen::Matrix3d currentAttitude = odom_orient_.toRotationMatrix();
    //             Eigen::Vector3d RPY = currentAttitude.eulerAngles(0,1,2);
    //             //cout << "currentyaw:" << RPY(0) << endl;
    //             desiredPose_.header.frame_id = "map";
    //             desiredPose_.header.stamp = pos_->header.stamp;
    //             desiredPose_.pose.position.x = desiredPos.x();
    //             desiredPose_.pose.position.y = desiredPos.y();
    //             desiredPose_.pose.position.z = desiredPos.z();
    //             break;
            
    //         }
    //     }
    // desiredStates_pub_.publish(cmd_);
    // desiredPose_pub_.publish(desiredPose_);

    // dir_ << cos(cmd_.yaw), sin(cmd_.yaw), 0;

    // drawCmd(desired_pos_, 2 * dir_, 1, Eigen::Vector4d(1, 1, 0, 0.7));
    // drawCmd(odom_pos_, 2 * dir_, 1, Eigen::Vector4d(1, 1, 0, 0.7));
}

void TrajectoryReplanNode::odomCallback(const nav_msgs::OdometryConstPtr &msg) {
    // odom_ = msg;
    odom_pos_(0) = msg->pose.pose.position.x;
    odom_pos_(1) = msg->pose.pose.position.y;
    odom_pos_(2) = msg->pose.pose.position.z;

    odom_vel_(0) = msg->twist.twist.linear.x;
    odom_vel_(1) = msg->twist.twist.linear.y;
    odom_vel_(2) = msg->twist.twist.linear.z;

    odom_orient_.w() = msg->pose.pose.orientation.w;
    odom_orient_.x() = msg->pose.pose.orientation.x;
    odom_orient_.y() = msg->pose.pose.orientation.y;
    odom_orient_.z() = msg->pose.pose.orientation.z;
    currentPose_.header.frame_id = "map";
    currentPose_.header.stamp = msg->header.stamp;
    currentPose_.pose.position = msg->pose.pose.position;
    currentPose_.pose.orientation = msg->pose.pose.orientation;
    currentPose_pub_.publish(currentPose_);
    // desiredStatesPub(); // 将minimum jerk的规划结果发布给几何控制器控制
    // displayTrajWithColor();
}

void TrajectoryReplanNode::posCallback(const geometry_msgs::PoseStampedConstPtr &msg) {
    pos_ = msg;
    odom_pos_(0) = msg->pose.position.x;
    odom_pos_(1) = msg->pose.position.y;
    odom_pos_(2) = msg->pose.position.z;
    // desiredStatesPub();
}

void TrajectoryReplanNode::velCallback(const airsim_ros::VelDataConstPtr &msg) {
    // odom_vel_(0) = msg->x;
    // odom_vel_(1) = msg->y;
    // odom_vel_(2) = msg->z;
}

void TrajectoryReplanNode::circlePosCallback(const airsim_ros::CirclePosesConstPtr &msg) {
    if (circle_info_init)
        {    
            circle_num_ = msg->poses.size() -1; // -1 忽略动圈
            circle_poses_.resize(circle_num_, 3);
            // cout << "waypoint number:" << waypoint_num_ << endl;
            for (int i = 0; i < circle_num_; ++i) { 
            circle_poses_(i,0) = msg->poses[i].position.x;
            circle_poses_(i,1) = msg->poses[i].position.y;
            circle_poses_(i,2) = msg->poses[i].position.z;
            }
            cout << "circle_poses_:" << circle_poses_ << endl;
            circle_count_ = 0;
            circle_target_pos_ = circle_poses_.row(circle_count_);
            circle_info_init = false;
        }
}

void TrajectoryReplanNode::circleDetectCallback(const custom_msgs::CircleRecogConstPtr& msg){
   circle_detect_pos_  << msg->point.x, msg->point.y, msg->point.z;
   circle_detect_normal_vec_ << msg->vector.x, msg->vector.y, msg->vector.z;
   cout << "circle_detect_pos: " << circle_detect_pos_ << endl;
   cout << "circle_detect_normal_vec_: " << circle_detect_normal_vec_ << endl;
}

void TrajectoryReplanNode::minijerkTrickerCallback(const airsim_ros::MinijerkTrickerConstPtr &msg){
    minijerk_tricker_info_ = msg;
    minijerk_tricker_ = msg->tricker;
    odom_vel_(0) = msg->current_vel.x;
    odom_vel_(1) = msg->current_vel.y;
    odom_vel_(2) = msg->current_vel.z;
    odom_acc_(0) = msg->current_acc.x;
    odom_acc_(1) = msg->current_acc.y;
    odom_acc_(2) = msg->current_acc.z;


    waypoint_num_ = 4; 
    waypoints_.resize(waypoint_num_, 3);
    waypoints_(0,0) = msg->current_pos.x;
    waypoints_(0,1) = msg->current_pos.y;
    waypoints_(0,2) = msg->current_pos.z;

    // if(scalar_ > 1.0){
        // waypoints_(1,0) = msg->target_pos.x;
        // waypoints_(1,1) = msg->target_pos.y;
        // waypoints_(1,2) = msg->target_pos.z;

    waypoints_(2,0) = msg->target_pos.x;
    waypoints_(2,1) = msg->target_pos.y;
    waypoints_(2,2) = msg->target_pos.z;

    Eigen::Vector3d start_p = waypoints_.row(0);
    Eigen::Vector3d end_p = waypoints_.row(2);

    double distance = (end_p - start_p).norm();
    Eigen::Vector3d vec = (end_p - start_p).normalized();

    Eigen::Vector3d lengthen_p_in = start_p +  vec * scalar_1 * distance;
    waypoints_(1,0) = lengthen_p_in(0);
    waypoints_(1,1) = lengthen_p_in(1);
    waypoints_(1,2) = lengthen_p_in(2);

    Eigen::Vector3d lengthen_p_out = start_p +  vec * scalar_2 * distance;
    waypoints_(3,0) = lengthen_p_out(0);
    waypoints_(3,1) = lengthen_p_out(1);
    waypoints_(3,2) = lengthen_p_out(2);
    // }
    // else {


        // Eigen::Vector3d start_p = waypoints_.row(0);
        // Eigen::Vector3d end_p = waypoints_.row(2);

        // double distance = (end_p - start_p).norm();
        // Eigen::Vector3d vec = (end_p - start_p).normalized();
        // Eigen::Vector3d lengthen_p = start_p +  vec * scalar_ * distance;
        // waypoints_(1,0) = lengthen_p(0);
        // waypoints_(1,1) = lengthen_p(1);
        // waypoints_(1,2) = lengthen_p(2);
    // }
}

void TrajectoryReplanNode::setWaypointsForMinijerk(Eigen::Vector3d start_p, Eigen::Vector3d end_p)
{
    waypoint_num_ = 4; 
    waypoints_.resize(waypoint_num_, 3);
    waypoints_.row(0) = start_p;
    waypoints_.row(2) = end_p;

    double distance = (end_p - start_p).norm();
    Eigen::Vector3d vec = (end_p - start_p).normalized();
    // 中间插点
    Eigen::Vector3d lengthen_p_in = start_p +  vec * scalar_1 * distance;
    waypoints_(1,0) = lengthen_p_in(0);
    waypoints_(1,1) = lengthen_p_in(1);
    waypoints_(1,2) = lengthen_p_in(2);
    // 延长线插点
    Eigen::Vector3d lengthen_p_out = start_p +  vec * scalar_2 * distance;
    waypoints_(3,0) = lengthen_p_out(0);
    waypoints_(3,1) = lengthen_p_out(1);
    waypoints_(3,2) = lengthen_p_out(2);
}


Eigen::VectorXd TrajectoryReplanNode::timeAllocation(const Eigen::MatrixXd &waypoints) {
    Eigen::VectorXd time(waypoints.rows() - 1);
    double current_vel = odom_vel_.norm();
    
        for (int i = 0; i < waypoints.rows() - 1; ++i) {
            double distance = (waypoints.row(i+1) - waypoints.row(i)).norm();
            double t = max_vel_ / max_acc_;
            double d = 0.5 * max_acc_ * t * t;
            if (distance < 2 * d) {
                time(i) = 2 * sqrt(distance / max_acc_);
            } else {
                time(i) = 2 * t + (distance - 2 * d) / max_vel_;
            }      
        }

        // time(0) = 0.6 * time(0);

        // if (current_vel != 0) {
        //     double distance = (waypoints.row(1) - waypoints.row(0)).norm();
        //     time(0) = distance  / current_vel;
        //     for (int i = 1; i < waypoints.rows() - 1; ++i) {
        //         double distance = (waypoints.row(i+1) - waypoints.row(i)).norm();
        //         time(i) = distance * 1.2 / max_vel_;
        //     }
        // }
    return time;
}


void TrajectoryReplanNode::displayTrajWithColor() {
  visualization_msgs::Marker points, line_strip;
  points.header.frame_id = line_strip.header.frame_id = "map";
  points.header.stamp = line_strip.header.stamp = ros::Time::now();
  
  points.ns = line_strip.ns = "traj";
  points.id = 0;
  line_strip.id = 1;

  points.action = line_strip.action = visualization_msgs::Marker::ADD;

  points.pose.orientation.w =line_strip.pose.orientation.w = 1.0;

  points.type = visualization_msgs::Marker::POINTS;
  line_strip.type = visualization_msgs::Marker::LINE_STRIP;

  points.scale.x = 0.5;
  points.scale.y = 0.5;

  line_strip.scale.x = 0.3;

  points.color.a = 1.0;
  points.color.g = 1.0;
  line_strip.color.a = 1.0;
  line_strip.color.b = 1.0;
  line_strip.color.g = 1.0;

  geometry_msgs::Point pt;
  Eigen::Vector3d pos;
  for (int i = 0; i < segment_num_; ++i) {
      for (double t = 0; t < times_(i); t += 0.01) {
          pos = trajPlanWaypoints_->getPosition(coeff_matrix_, i, t);
          pt.x = pos(0);
          pt.y = pos(1);
          pt.z = pos(2);
          line_strip.points.push_back(pt);
      }
    }
  for (int i = 0; i < waypoint_num_; ++i) {
      pt.x = waypoints_(i,0);
      pt.y = waypoints_(i,1);
      pt.z = waypoints_(i,2);
      points.points.push_back(pt);
  }
  traj_vis_pub_.publish(points);
  traj_vis_pub_.publish(line_strip);

  ros::Duration(0.001).sleep();
}

void TrajectoryReplanNode::drawCmd(const Eigen::Vector3d& pos, const Eigen::Vector3d& vec, const int& id,
             const Eigen::Vector4d& color) {
  visualization_msgs::Marker mk_state;
  mk_state.header.frame_id = "map";
  mk_state.header.stamp = ros::Time::now();
  mk_state.id = id;
  mk_state.type = visualization_msgs::Marker::ARROW;
  mk_state.action = visualization_msgs::Marker::ADD;

  mk_state.pose.orientation.w = 1.0;
  mk_state.scale.x = 0.1;
  mk_state.scale.y = 0.2;
  mk_state.scale.z = 0.3;

  geometry_msgs::Point pt;
  pt.x = pos(0);
  pt.y = pos(1);
  pt.z = pos(2);
  mk_state.points.push_back(pt);

  pt.x = pos(0) + vec(0);
  pt.y = pos(1) + vec(1);
  pt.z = pos(2) + vec(2);
  mk_state.points.push_back(pt);

  mk_state.color.r = color(0);
  mk_state.color.g = color(1);
  mk_state.color.b = color(2);
  mk_state.color.a = color(3);

  cmd_vis_pub_.publish(mk_state);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "trajectory_replan_node");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");
    //namedWindow("depth2gray");
  
    TrajectoryReplanNode trajectory_replan_node(nh, nh_private);

    ros::spin();
    return 0;
}